#include "sxcom.h"

int fd;		// Defines fd
uint8_t CUBEID;	// defines CUBEID

int initSXCOM(uint8_t ID)
{
	CUBEID = ID; 
	fd = serialOpen(SER_PORT, BAUD_RATE); 
	return fd; 
} 

xMsg * createMessage(uint8_t num, uint8_t to, uint8_t from, uint8_t size, uint8_t * data)
{
	xMsg * tempMsg = new xMsg; 
	tempMsg->callCard = CALL_CARD; 
	tempMsg->msgNum = num; 
	tempMsg->toAddr = to; 
	tempMsg->fromAddr = from; 
	tempMsg->size = size; 		
	tempMsg->confirmation = 0x00;				// 0 is a message. 0xAA is confirmation

	uint8_t tempHeader[HEADER_SIZE];			// Holds header temporarily while hashing
	tempHeader[0] = tempMsg->callCard; 
	tempHeader[1] = tempMsg->msgNum; 
	tempHeader[2] = tempMsg->toAddr; 
	tempHeader[3] = tempMsg->fromAddr; 
	tempHeader[4] = tempMsg->size; 		
	tempHeader[5] = tempMsg->confirmation;
	//memset(tempMsg->headerDigest, 0x00, DIGEST_SIZE);	// Sets to 0
	MD4(tempHeader, HEADER_SIZE, tempMsg->headerDigest);	// Makes header digest
	
//	for(int few=0; few<DIGEST_SIZE; few++)
//		std::cout << (int)tempMsg->headerDigest[few];
//	std::cout << std::endl; 

	memset(tempMsg->messageBody, 0x00, MAX_MSG_SIZE); 	// Sets all memory body to 0
	memcpy(tempMsg->messageBody, data, size);		// Copies data into message body
	MD4(tempMsg->messageBody, MAX_MSG_SIZE,  tempMsg->messageBodyDigest);	// Creates digest and puts it into struct

	// DEBUGGING
	//for(int ll=0; ll<DIGEST_SIZE; ll++)
	//	std::cout << (int)tempMsg->messageBodyDigest[ll];

	return tempMsg; 
}

xMsg * createGoodConfirmation(uint8_t to, uint8_t from)
{
	xMsg * tempMsg = new xMsg; 
	tempMsg->callCard = CALL_CARD;
	tempMsg->msgNum = 0;		// Doesn't matter for a confirmation 
	tempMsg->toAddr = to; 
	tempMsg->fromAddr = from; 
	tempMsg->size = 0x00; 		
	tempMsg->confirmation = 0xAA;	// 0xAA is confirmation

	uint8_t tempHeader[HEADER_SIZE];			// Holds header temporarily while hashing
	tempHeader[0] = tempMsg->callCard; 
	tempHeader[1] = tempMsg->msgNum; 
	tempHeader[2] = tempMsg->toAddr; 
	tempHeader[3] = tempMsg->fromAddr; 
	tempHeader[4] = tempMsg->size; 		
	tempHeader[5] = tempMsg->confirmation;
	memset(tempMsg->headerDigest, 0x00, DIGEST_SIZE);	// Sets to 0
	MD4(tempHeader, HEADER_SIZE, tempMsg->headerDigest);	// Makes header digest
	return tempMsg; 
}


xMsg * createBadConfirmation(uint8_t to, uint8_t from)
{
	xMsg * tempMsg = new xMsg; 
	tempMsg->callCard = CALL_CARD;
	tempMsg->msgNum = 0;		// Doesn't matter for a confirmation 
	tempMsg->toAddr = to; 
	tempMsg->fromAddr = from; 
	tempMsg->size = 0x00; 		
	tempMsg->confirmation = 0xFF;	// 0xFF means message was bad
	uint8_t tempHeader[HEADER_SIZE];			// Holds header temporarily while hashing
	tempHeader[0] = tempMsg->callCard; 
	tempHeader[1] = tempMsg->msgNum; 
	tempHeader[2] = tempMsg->toAddr; 
	tempHeader[3] = tempMsg->fromAddr; 
	tempHeader[4] = tempMsg->size; 		
	tempHeader[5] = tempMsg->confirmation;
	memset(tempMsg->headerDigest, 0x00, DIGEST_SIZE);	// Sets to 0
	MD4(tempHeader, HEADER_SIZE, tempMsg->headerDigest);	// Makes header digest

	return tempMsg; 
}

int msgSend(xMsg * msg)
{
	// Send the message
	for(int i=0; i<sizeof(xMsg); i++)
	{
		serialPutchar(fd, ((uint8_t *)((void *)msg))[i]); 
	}
	
	// Wait for response
	// delay
	usleep(100000); 

	if(msg->confirmation == 0x00)
	{
		if(goodSend())
			return 0; 
		else{
			std::cout << "resending" << std::endl; 
			msgSend(msg);
			return 0; 
		}
	}
	else
		return 0; 
	
}

bool goodSend()
{
	uint8_t remMsgNum;
	uint8_t remToAddr;
	uint8_t remFromAddr;
	uint8_t remSize;
	uint8_t remConfirmation;
	uint8_t  remHeaderBuff[6];
	uint8_t  localHeaderDigest[DIGEST_SIZE]; 
	uint8_t  remHeaderDigest[DIGEST_SIZE]; 

	//bool keepTrying = true;
	
	//sleep(1);	// IF BECOMES TO SLOW TAKE THIS OUT
	 
	for(int i=0; i<MAX_MSG_SIZE*100; i++)
	{
			//keepTrying = false; 
			if(serialGetchar(fd) == CALL_CARD) 
			{
				remMsgNum = serialGetchar(fd);
				remToAddr = serialGetchar(fd);
				remFromAddr = serialGetchar(fd);
				remSize = serialGetchar(fd);
				remConfirmation = serialGetchar(fd); 
				for(int hd=0; hd < DIGEST_SIZE; hd++)
					remHeaderDigest[hd] = serialGetchar(fd); 
				
				uint8_t tempCall = CALL_CARD;	
				memcpy(remHeaderBuff, &tempCall, 1); 
				memcpy(remHeaderBuff+1, &remMsgNum, 1); 
				memcpy(remHeaderBuff+2, &remToAddr, 1); 
				memcpy(remHeaderBuff+3, &remFromAddr, 1); 
				memcpy(remHeaderBuff+4, &remSize, 1);
				memcpy(remHeaderBuff+5, &remConfirmation, 1); 
				MD4(remHeaderBuff, HEADER_SIZE, localHeaderDigest); 
				int res1 = memcmp(localHeaderDigest, remHeaderDigest, HEADER_SIZE); 

				// HEADER ISOK, the message belongs to this cube and the message is not a confirmation
				//std::cout << "res1: " << res1 << " remToAddr: " << remToAddr << " conf" << remConfirmation << std::endl; 
			
				// Get rid of excess baggage
				//for(int b=0; b<(MAX_MSG_SIZE+DIGEST_SIZE); b++)
				//	serialGetchar(fd);
				if((res1 == 0) && (remToAddr == CUBEID) && (remConfirmation != 0x00))	
				{
					if(remConfirmation == 0xAA)
						return true; 
					else
						return false; 
				}
				
			}
		
	}
	
	//for(int b=0; b<(MAX_MSG_SIZE+DIGEST_SIZE); b++)
	//	serialGetchar(fd);
	return false; 	// No confirmation received
}

uint8_t * msgRec(uint8_t * retSize)
{
	*retSize = 0; 
	uint8_t remMsgNum;
	uint8_t remToAddr;
	uint8_t remFromAddr;
	uint8_t remSize;
	uint8_t remConfirmation;
	uint8_t remHeaderBuff[6];
	uint8_t localHeaderDigest[DIGEST_SIZE]; 
	uint8_t remHeaderDigest[DIGEST_SIZE]; 
	uint8_t * remMessageBody; 
	uint8_t remMessageDigest[DIGEST_SIZE];
	uint8_t localMessageDigest[DIGEST_SIZE]; 

	//std::cout << "debugging" << std::endl; 

	bool keepTrying = true; 
	for(int i=0; i<100 && keepTrying; i++)
	{
		//std::cout << "in loop" << std::endl; 
		//std::cout << serialDataAvail(fd); 
		if(serialDataAvail(fd) > 0)	// Maybe make it more than 0 just in case
		{
			//std::cout << "DataAvailable" << std::endl; 
			keepTrying = false; 
			if(serialGetchar(fd) == CALL_CARD) 
			{
				std::cout << "Got a Call Card" << std::endl; 
				remMsgNum = serialGetchar(fd);
				remToAddr = serialGetchar(fd);
				remFromAddr = serialGetchar(fd);
				remSize = serialGetchar(fd);
				remConfirmation = serialGetchar(fd); 
				for(int hd=0; hd < DIGEST_SIZE; hd++)
					remHeaderDigest[hd] = serialGetchar(fd); 
				
				uint8_t tempCall = CALL_CARD;	
				memcpy(remHeaderBuff, &tempCall, 1); 
				memcpy(remHeaderBuff+1, &remMsgNum, 1); 
				memcpy(remHeaderBuff+2, &remToAddr, 1); 
				memcpy(remHeaderBuff+3, &remFromAddr, 1); 
				memcpy(remHeaderBuff+4, &remSize, 1);
				memcpy(remHeaderBuff+5, &remConfirmation, 1); 
				MD4(remHeaderBuff, HEADER_SIZE, localHeaderDigest); 
				int res1 = memcmp(localHeaderDigest, remHeaderDigest, HEADER_SIZE); 
			
				//for(int few=0; few<DIGEST_SIZE; few++)
				//	std::cout<< (uint8_t)localHeaderDigest[few];	
				//std::cout << "Res1: " << res1 << std::endl; 
				
				// HEADER ISOK, the message belongs to this cube and the message is not a confirmation
				if((res1 == 0) && (remToAddr == CUBEID) && (remConfirmation == 0x00))	
				{
					// Maybe wait for more data here? 
					remMessageBody = new uint8_t[MAX_MSG_SIZE];	// Body stored here
					memset(remMessageBody, 0x00, MAX_MSG_SIZE);	// Clears it

					for(int j=0; j<MAX_MSG_SIZE; j++)
						remMessageBody[j] = serialGetchar(fd); 

					MD4(remMessageBody, MAX_MSG_SIZE, localMessageDigest); 
					
					// DEBUGGING
					//for(int xk=0; xk<DIGEST_SIZE; xk++)
					//	std::cout<< (int)localMessageDigest[xk];
					
					for(int j=0; j<DIGEST_SIZE; j++)
						remMessageDigest[j] = serialGetchar(fd); 	
	
					//MD4(remMessageBody, MAX_MSG_SIZE, localMessageDigest); 
					
					int res2 = memcmp(localMessageDigest, remMessageDigest, HEADER_SIZE); 

					// Success!
					if(res2 == 0)
					{
						// Send confirmation
						std::cout << "Received OK" << std::endl; 
						msgSend(createGoodConfirmation(remFromAddr, remToAddr)); 
						return remMessageBody; 
					}
					else
					{
						// Send failure
						
						std::cout << "Failed to Received, asking for another" << std::endl; 
						msgSend(createBadConfirmation(remFromAddr, remToAddr)); 
						return NULL; 
					}
				}
			}
		}
	}
}
